#include "flamingdeath.h"
#include "formatted_io.h"
#include "kalman.h"
#include "navigation.h"
#include "delay.h"
#include <math.h>

struct waypoint {
  float x;
  float y;
};

class flamingdeath {
private:
  /* accelerometer data */
  float accel_data[3];
  //float x, y, z;

  /* gps data */
  long latitude, longitude;
  float gps_dir;
  float gps_speed;

  /* read id */
  uint32_t last_accel_read_id;
  uint32_t last_gps_read_id;

  /* calibration values for accelerometer */
  float x_mul;
  float x_off;
  float y_mul;
  float y_off;
  float z_mul;
  float z_off;

  /* calibration values for compass */
  float dir_off;
  
  /* list of waypoints */
  waypoint waypoints[40];
  uint8_t n_waypoints;

  /* formatted_io block for Bluetooth */
  formatted_io BT;

public:
  flamingdeath() :
    last_accel_read_id(4134434234),
    last_gps_read_id(4025328523),
    x_mul(0.0f),
    x_off(0.0f),
    y_mul(0.0f),
    y_off(0.0f),
    z_mul(0.0f),
    z_off(0.0f),
    dir_off(-9.0f),
    n_waypoints(0),
    BT(flamingdeath_BTSend, flamingdeath_BTRecv, flamingdeath_BTRecvReady)
  {
    accel_data[0] = 0.0f;
    accel_data[1] = 0.0f;
    accel_data[2] = 0.0f;
    // initialize gps parser
    gps_init(&gps);
  }

  /**
   * Reads from the accelerometer if new data is available.  It applies calibration
   * and places the results in x, y, and z.  It returns true if new accelerometer
   * data is available, false otherwise.
   */
  bool read_accel()
  {
    if(last_accel_read_id != accel_read_id) {
      uint32_t x_raw, y_raw, z_raw;
      // spin until we read all values without the read id changing
      while(last_accel_read_id != accel_read_id) {
        last_accel_read_id = accel_read_id;
        x_raw = accel_x_raw;
        y_raw = accel_y_raw;
        z_raw = accel_z_raw;
      }

      // apply calibration
      accel_data[0] = (float)accel_x_raw * x_mul + x_off;
      accel_data[1] = (float)accel_y_raw * y_mul + y_off;
      accel_data[2] = (float)accel_z_raw * z_mul + z_off;
      return true;
    }
    return false;
  }


  /**
   * Reads from the gps if new data is available.  It returns true if new gps data
   * is available, false otherwise
   */
  bool read_gps()
  {
    if(last_gps_read_id != gps.id) {
      while(last_gps_read_id != gps.id) {
        last_gps_read_id = gps.id;
        latitude = gps.latitude;
        longitude = gps.longitude;
        gps_dir = gps.course;
        gps_speed = gps.speed;
      }
      return true;
    }
    return false;
  }

  /**
   * Reads the compass direction and applies calibration.  Return values is the compass
   * direction in radians after calibration
   */
  float read_compass()
  {
    return ((float)compass_direction_raw / 10.0f + dir_off) * M_PI/180.0f;
  }

  void run()
  {
    BT.print("Flaming Death version 0.0.0.0.1\r\n");

	  // input and state information
	  kal_t Px[9]       // 3x3 error covariance
	  = { 0.144973, 0.234941, 0.083876,
		  0.227745, 0.380634, 0.139176,
		  0.079498, 0.124546, 0.124546 };
	  kal_t Py[9]       // 3x3 error covariance
	  = { 0.144973, 0.234941, 0.083876,
		  0.227745, 0.380634, 0.139176,
		  0.079498, 0.124546, 0.124546 };
	  
	  kal_t Kx[9];      // 3x3 kalman gain
	  kal_t Ky[9];      // 3x3 kalman gain
	  // {acceleration, velocity, position}
	  kal_t kalIn_x[3] = {0.0f, 0.0f, 0.0f};
	  kal_t kalIn_y[3] = {0.0f, 0.0f, 0.0f};
	  kal_t kalOut_x[3] = {0.0f, 0.0f, 0.0f};
	  kal_t kalOut_y[3] = {0.0f, 0.0f, 0.0f};

	  kal_t ux[1] = {0.0f};       // commanded input {acceleration}
	  kal_t uy[1] = {0.0f};       // commanded input {acceleration}
	  
	  // base line gps as lat/long
	  float gps_start[2] = {0.0f, 0.0f};
	  float acc_xy[2] = {0.0f, 0.0f};
    float direction = 0.0f;
	
	  
    while(true) {
      bool run_navigation = false;

      if(read_accel()) {
        // handle accelerometer read
        direction = read_compass();
        
        convert_accelerometer(direction, accel_data, acc_xy);
		
		  kalIn_x[0] = 0;//acc_xy[0];
		  kalIn_y[0] = 0;//acc_xy[0];
		  run_navigation = true;
      }

      if(read_gps()) {
        // handle gps read
        kalIn_x[1] = extract_x_comp(direction, gps_speed);
        kalIn_y[1] = extract_y_comp(direction, gps_speed);
		  
		  kalIn_x[2] = diff_lat_meters(gps_start[0],latitude);
		  kalIn_y[2] = diff_lon_meters(gps_start[1],longitude);
		  
		  run_navigation = true;
      }
      
		if (run_navigation) {
			// kalman
			kalman(Px,Kx,kalIn_x,kalOut_x,ux,1);
			kalman(Py,Ky,kalIn_y,kalOut_y,uy,1);
		}
		
		BT.printf("x acc: %f\t\ty acc: %f\r\n",kalOut_x[0],kalOut_y[0]);
		BT.printf("x vel: %f\t\ty vel: %f\r\n",kalOut_x[1],kalOut_y[1]);
		BT.printf("x pos: %f\t\ty pos: %f\r\n",kalOut_x[2],kalOut_y[2]);
		BT.print("\n\r");
		
      // if a button press happens
      if(flamingdeath_ButtonRead()) {
        BT.print("Read start\r\n");
		
		while (!read_gps()); 
		// handle gps read
		  gps_start[0] = latitude;
		  gps_start[1] = longitude;
		  
		  kalIn_x[0] = kalIn_x[1] = kalIn_x[2] = 0.0f;
		  kalIn_y[0] = kalIn_y[1] = kalIn_y[2] = 0.0f;
		  
		  kalOut_x[0] = kalOut_x[1] = kalOut_x[2] = 0.0f;
		  kalOut_y[0] = kalOut_y[1] = kalOut_y[2] = 0.0f;
		  
		  Px[0] = 0.144973;
		  Px[1] = 0.234941;
		  Px[2] = 0.083876;
		  Px[3] = 0.227745;
		  Px[4] = 0.380634;
		  Px[5] = 0.139176;
		  Px[6] = 0.079498; 
		  Px[7] = 0.124546;
		  Px[8] = 0.124546;
		  
		  Py[0] = 0.144973;
		  Py[1] = 0.234941;
		  Py[2] = 0.083876;
		  Py[3] = 0.227745;
		  Py[4] = 0.380634;
		  Py[5] = 0.139176;
		  Py[6] = 0.079498;
		  Py[7] = 0.124546;
		  Py[8] = 0.124546;
      }


    }


  }

 void gps_debug() {
	  BT.print("Read start\r\n");
	 while (1) {
		 if(read_gps()) {
			 // handle gps read
			 float direction = read_compass();
			 
			 BT.print(direction,6);
			 BT.print(", ");
			 BT.print(gps_speed,6);
			 BT.print(", ");
			 BT.print((int)latitude,10 );
			 BT.print(", ");
			 BT.print((int)longitude,10);
			 BT.print(", ");
			 BT.print((float)extract_x_comp(direction, gps_speed),6);
			 BT.print(", ");
			BT.print((float)extract_y_comp(direction, gps_speed),6);
			 BT.print(", ");
			 BT.print("\r\n");
		 }
	 }
 }
};

int main()
{
  flamingdeath_Init();

  flamingdeath fd;

  //fd.run();
	fd.gps_debug();

}
